/*
 * @Author: Mcfly
 * @Date: 2021-07-25 20:36:28
 * @LastEditTime: 2021-08-02 15:14:38
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\Ctrl\Height.c
 * 本人github:https://github.com/McflyWZX
 */
#include "Height.h"
#include "stdlib.h"

Height *Height_Init(void)
{
	Height *height = (Height *)malloc(sizeof(Height));
	height->h = 0;
	height->speed = 0;
	height->deltaT_Tof = 1 / 10.0f;
	height->speedTof = 0;
	height->HTofLast = 0;
	height->deltaT_Az = 1 / 200.0f;
	height->azLast = 0;
	height->speedTofLast = 0;
	return height;
}


float fastCos(float angle)
{
	angle /= (180.0f / 3.1415926535f);
	return 1 - 0.5f * angle * angle;
}

static float nowCosA2;
void Height_UpdateFrame(Height *height, Atti *nowAtti)
{
	float nowCosA = fastCos(nowAtti->pitch) * fastCos(nowAtti->roll);
	nowCosA2 = nowCosA * nowCosA;
	float az = 0.4f * (nowAtti->az - 9.8f * nowCosA2) + 0.6f * height->azLast;
	height->azLast = az;
    height->speed = (az * height->deltaT_Az) + height->speed;
    height->h = (height->speed * height->deltaT_Az) + height->h;
}

void Height_UpdateTofSpeed(Height *height, float tofHeight, float realDeltaTime)
{
	//高度也做滤波
	float tofHeightTemp = 0.3f * height->HTofLast + 0.7f * tofHeight * nowCosA2;
	height->speedTof = 0.6f * (tofHeightTemp - height->HTofLast) / realDeltaTime + 0.4f * height->speedTofLast;
	height->speed = 0.1f * height->speedTof + 0.9f * height->speed;
	height->h = 0.2f * tofHeightTemp + 0.8f * height->h;
	height->speedTofLast = height->speedTof;
	height->HTofLast = tofHeightTemp;
}
